#include <Eigen/Dense>
#include <Eigen/Core>
#include <iostream>
#include <cmath>

#define DEG2RAD(x) ((x) * M_PI / 180.0)

int main() {
    // 机器人初始位置
    Eigen::Vector3d P(1, 0, 0);  // 初始位置 {1, 0, 0}
    Eigen::Matrix3d V = Eigen::Matrix3d::Identity();  // 初始姿态矩阵为单位矩阵
    
    // 第一步：绕原点旋转到 θ=60° 的位置
    double theta = DEG2RAD(60);  // 转换为弧度
    Eigen::Matrix3d R;
    R << std::cos(theta), 0, std::sin(theta),
         0,              1, 0,
         -std::sin(theta), 0, std::cos(theta);
    
    // 计算旋转后的新位置
    Eigen::Vector3d P_rotated = R * P;
    std::cout << "Position after rotation: " << P_rotated.transpose() << std::endl;
    
    // 第二步：沿斜面运动
    double beta = DEG2RAD(30);  // 斜面倾角 β=30°
    Eigen::Vector3d slide_direction(std::cos(beta), 0, -std::sin(beta));  // 沿斜面运动的方向
    
    // 沿斜面移动 1 个单位长度
    Eigen::Vector3d P_final = P_rotated + slide_direction;
    std::cout << "Final position after sliding: " << P_final.transpose() << std::endl;

    // 输出最终姿态矩阵（在这个例子中，姿态没有发生变化）
    std::cout << "Final orientation (no change): \n" << V << std::endl;

    return 0;
}
